#define _USE_MATH_DEFINES

#define WAYPOINT 1
#define HEADING 2
#define HALT 3
#define ATSPEED 4
#define ATHEADINGSPEED 5
#define ATHEADINGSPEEDMIXED 6

#define MAX_LINE 256
#define MAX_ARGS 4

#define secsPerLog 1


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <signal.h>
#include <time.h>

#include "maestro.h"
#include "compass.h"
#include "gotemp.h"
#include "gps_boat.h"
#include "config.h"
#include <gps.h>
#include "main.h"

struct boat{
	int state;
	bool commandStarted;
	int command;
	double commandArgs[MAX_ARGS];
	gps_data_t current;
	double retHeading, retPitch, retRoll, retTemp;
	bool goodData;
	double tempProbe;
};


boat myBoat;

int maestro;
int compass;
FILE * logFile;
FILE * statusFile;





int init(){
	fprintf(stderr, "Initializing.\n");
	signal(SIGINT, signal_handler);
	maestro = maestro_connect();
	compass = compass_connect();
	fprintf(stderr, "M:%d C:%d\n", maestro, compass);

	if ((maestro < 0) || (compass < 0))
	{
		fprintf(stderr, "Maestro or compass connection error, exiting. M: %d, C: %d\n", maestro, compass);
		exit(1);
	}

	gotemp_open();

	double retHeading, retPitch, retRoll, retTemp;
	bool goodData;
	get_compass(&retHeading, &retPitch, &retRoll, &retTemp, &goodData);
	fprintf(stderr, "\t\tHeading: %f, Pitch: %f, Roll: %f, Temp: %f, goodData: %d\n", retHeading, retPitch, retRoll, retTemp, goodData);
	gps_connect();

	fprintf(stderr, "\t\tDevices connected, loading configuration.\n");

	loadConfig();
	fprintf(stderr, "\t\tConfiguration loaded.\n");
	myBoat.state = -1;
	myBoat.commandStarted = false;
	char string[128];
	sprintf(string, "Outputs/log-%ld.txt", time(NULL));
	logFile = fopen(string, "w");
	return 0;
}


int main(){
	init();
	bool hitWaypoint = false;
	while (1)
	{
		if (getCommand()){
			fprintf(stderr, "\t\t Parsing new command.\n");
			// parse new command
			parseCommand();
		}
		fprintf(stderr, "Compass.\n");
		get_compass(&myBoat.retHeading, &myBoat.retPitch, &myBoat.retRoll, &myBoat.retTemp, &myBoat.goodData);
		
		fprintf(stderr, "GPS.\n");
		myBoat.current = gps_fetch();
		
		myBoat.tempProbe = -200;//gotemp_read();
		fprintf(stderr, "Log.\n");
		if ((time(NULL) % secsPerLog) == 0){
			
			printLog();
		}
		printStatus();
		switch(myBoat.state){
			case WAYPOINT: {
				
				if (myBoat.commandStarted == false) {fprintf(logFile, "<waypoint>"); myBoat.commandStarted = true;}
				
				// Check GPS validity
				if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
					fprintf(stderr, "Grabbing GPS fix");
					break;
				}

				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				if (distance < myBoat.commandArgs[2])
				{
					fprintf(stderr, "At waypoint.\n");
					fprintf(logFile, "<comment>At waypoint.</comment>\n</wayPoint>\n");
					myBoat.commandStarted = false;
					myBoat.state = -1;
					break;
				}
				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - myBoat.commandArgs[3]*getRudderCoeff()*headingDiff;
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;


				unsigned int commandThrottle = 1500 + myBoat.commandArgs[3]*getThrottleCoeff()*distance;
				if (commandThrottle >= 2000)
					commandThrottle = 2000;
				else if (commandThrottle <= 1000)
					commandThrottle = 1000;
				
				setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				fprintf(stderr, "Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", distance, headingDiff, commandThrottle, commandRudder);
				fprintf(logFile, "<comment>Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n", distance, headingDiff, commandThrottle, commandRudder);
				}
				break;

			case HEADING: {
			
				if (myBoat.commandStarted == false) {fprintf(logFile, "<heading>"); myBoat.commandStarted = true;}
				double desiredHeading = myBoat.commandArgs[0];
				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder > 2000)
					commandRudder = 2000;
				else if (commandRudder < 1000)
					commandRudder = 1000;
				
				setTarget(maestro, getRudder(), commandRudder);

				if (headingDiff < 5)
				{
					setTarget(maestro, getRudder(), 1500);
					myBoat.commandStarted = false;
					fprintf(logFile, "</heading>");
					myBoat.state = -1;
				}

				}
				break;

			case HALT: {
				if (myBoat.commandStarted == false) {fprintf(logFile, "<halt>"); myBoat.commandStarted = true;}
				setTarget(maestro, getRudder(), 1500);
				setTarget(maestro, getThrottle(), 1500);

				}
				break;
			case ATSPEED: {
				
				if (myBoat.commandStarted == false) {fprintf(logFile, "<atspeed>"); myBoat.commandStarted = true;}
				
				// Check GPS validity
				if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
					fprintf(stderr, "Grabbing GPS fix");
					break;
				}

				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;


				unsigned int commandThrottle = 1500 + myBoat.commandArgs[2];
				if (commandThrottle >= 2000)
					commandThrottle = 2000;
				else if (commandThrottle <= 1000)
					commandThrottle = 1000;
				
				setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				fprintf(stderr, "Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", distance, headingDiff, commandThrottle, commandRudder);
				fprintf(logFile, "<comment>Distance to waypoint: %lf. Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n", distance, headingDiff, commandThrottle, commandRudder);
				}
				break;


			case ATHEADINGSPEED: {
				// myBoat.commandArgs[0] is heading, commadArgs[1] is speed
				if (myBoat.commandStarted == false) {fprintf(logFile, "<atheadingspeed>"); myBoat.commandStarted = true;}
				
				
				// Check GPS validity
				/*if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
					fprintf(stderr, "Grabbing GPS fix");
					break;
				}
				
				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				if (distance < myBoat.commandArgs[2])
				{
					fprintf(stderr, "At waypoint.\n");
					fprintf(logFile, "<comment>At waypoint.</comment>\n</wayPoint>\n");
					myBoat.commandStarted = false;
					myBoat.state = -1;
					break;
				}
				*/

				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = myBoat.commandArgs[0]; //calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;


				unsigned int commandThrottle = 1500 + myBoat.commandArgs[1];
				if (commandThrottle >= 2000)
					commandThrottle = 2000;
				else if (commandThrottle <= 1000)
					commandThrottle = 1000;
				
				setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				fprintf(stderr, "Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", headingDiff, commandThrottle, commandRudder);
				fprintf(logFile, "<comment> Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n",  headingDiff, commandThrottle, commandRudder);
				}
				break;

			case ATHEADINGSPEEDMIXED: {
				// myBoat.commandArgs[0] is heading, commadArgs[1] is speed
				if (myBoat.commandStarted == false) {fprintf(logFile, "<atheadingspeedmixed>"); myBoat.commandStarted = true;}
				
				
				// Check GPS validity
				/*if (myBoat.current.status == 0 || ((myBoat.current.fix.latitude == 180.0) && (myBoat.current.fix.longitude == 0.0))){
					fprintf(stderr, "Grabbing GPS fix");
					break;
				}
				
				// Secondary check to ensure that we have real distances
				double distance = calcDistance(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);
				if (isnan(distance)){	
					fprintf(stderr, "Still isnan distance.\n");	
					break;
				}

				if (distance < myBoat.commandArgs[2])
				{
					fprintf(stderr, "At waypoint.\n");
					fprintf(logFile, "<comment>At waypoint.</comment>\n</wayPoint>\n");
					myBoat.commandStarted = false;
					myBoat.state = -1;
					break;
				}
				*/

				//double diffLat =  myBoat.current.fix.latitude - myBoat.commandArgs[0];
				//double diffLon =  myBoat.current.fix.longitude - myBoat.commandArgs[1];
				//fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
				double desiredHeading = myBoat.commandArgs[0]; //calcHeading(myBoat.current.fix.latitude, myBoat.current.fix.longitude, myBoat.commandArgs[0], myBoat.commandArgs[1]);

				double headingDiff = desiredHeading - myBoat.retHeading;
				if (headingDiff < -180)
					headingDiff = headingDiff + 360;
				if (headingDiff > 180)
					headingDiff = headingDiff - 360;

				unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
				if (commandRudder >= 2000)
					commandRudder = 2000;
				else if (commandRudder <= 1000)
					commandRudder = 1000;


				unsigned int commandThrottle = 1500 + myBoat.commandArgs[1];
				if (commandThrottle >= 2000)
					commandThrottle = 2000;
				else if (commandThrottle <= 1000)
					commandThrottle = 1000;
				
				// if (abs(ruddur-neutral) + abs(throttle-neutral) > 750)
				double rudderScale = abs(commandRudder - 1500) / 500;
				
				commandThrottle = (1-rudderScale) * (commandThrottle - 1500) + 1500;

				setTarget(maestro, getRudder(), commandRudder);
				usleep(1000);
				setTarget(maestro, getThrottle(), commandThrottle);
	

				fprintf(stderr, "Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.\n", headingDiff, commandThrottle, commandRudder);
				fprintf(logFile, "<comment> Heading difference: %lf. Commanded throttle: %d. Commanded rudder: %d.</comment>\n",  headingDiff, commandThrottle, commandRudder);
				}
				break;



			default: {
				setTarget(maestro, getRudder(), 1500);
				usleep(1000);
				setTarget(maestro, getThrottle(), 1500);

				}
				break;

	
		}
	}

}

void printLog()
{
	fprintf(stderr, "Printing Log.\n");

	fprintf(logFile, "<status>\n");
	time_t rawtime;
	struct tm * timeinfo;
	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	fprintf(logFile, "\t<compTime>%s</compTime>\n", asctime(timeinfo));
	
	time_t unixTime = myBoat.current.fix.time;
	fprintf(logFile, "\t<gpsTime>%s</gpsTime>\n", asctime(localtime(&unixTime)));

	switch(myBoat.state)
	{		
		case WAYPOINT:
			fprintf(logFile, "\t<state>Waypoint</state>\n");
			fprintf(logFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(logFile, "\t\t<commandPrecision>%lf</commandPrecision>\n", myBoat.commandArgs[2]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[3]); 
			break;
		case HEADING:
			fprintf(logFile, "\t<state>Heading</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[1]);
			break;
		case HALT:
			fprintf(logFile, "\t<state>Halt</state>\n");
			break;
		case ATSPEED:
			fprintf(logFile, "\t<state>At Speed</state>\n");
			fprintf(logFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[2]); 
			break;
		case ATHEADINGSPEED:
			fprintf(logFile, "\t<state>At Heading and Speed</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(logFile, "\t<state>At Heading and Speed, mixed</state>\n");
			fprintf(logFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(logFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
		default:
			fprintf(logFile, "\t<state>Waiting</state>\n");
			break;
	}
	switch(myBoat.commandStarted)
	{
		case true: fprintf(logFile, "\t<started>1</started>\n"); break;
		case false: fprintf(logFile, "\t<started>0</started>\n"); break;
	}
	fprintf(logFile, "\t<lat>%lf</lat>\n", myBoat.current.fix.latitude);
	fprintf(logFile, "\t<lon>%lf</lon>\n", myBoat.current.fix.longitude);
	fprintf(logFile, "\t<compass>%lf</compass>\n", myBoat.retHeading);
	fprintf(logFile, "\t<temp>%lf</temp>\n", myBoat.tempProbe);

	fprintf(logFile, "</status>\n");

	fprintf(stderr, "Wrote to log.\n");
}

void printStatus()
{
	fprintf(stderr, "Printing status.\n");

	
	statusFile = fopen("Outputs/status.txt", "w");	
	fprintf(statusFile, "<status>\n");
	time_t rawtime;
	struct tm * timeinfo;
	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	fprintf(statusFile, "\t<compTime>%s</compTime>\n", asctime(timeinfo));

	time_t unixTime = myBoat.current.fix.time;
	fprintf(statusFile, "\t<gpsTime>%s</gpsTime>\n", asctime(localtime(&unixTime)));

	switch(myBoat.state)
	{		
		case WAYPOINT:
			fprintf(statusFile, "\t<state>Waypoint</state>\n");
			fprintf(statusFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(statusFile, "\t\t<commandPrecision>%lf</commandPrecision>\n", myBoat.commandArgs[2]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[3]); 
			break;
		case HEADING:
			fprintf(statusFile, "\t<state>Heading</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[1]);
			break;
		case HALT:
			fprintf(statusFile, "\t<state>Halt</state>\n");
			break;
		case ATSPEED:
			fprintf(statusFile, "\t<state>At Speed</state>\n");
			fprintf(statusFile, "\t\t<commandLat>%lf</commandLat>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandLon>%lf</commandLon>\n", myBoat.commandArgs[1]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[2]); 
			break;
		case ATHEADINGSPEED:
			fprintf(statusFile, "\t<state>At Heading and Speed</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(statusFile, "\t<state>At Heading and Speed, mixed</state>\n");
			fprintf(statusFile, "\t\t<commandHeading>%lf</commandHeading>\n", myBoat.commandArgs[0]);
			fprintf(statusFile, "\t\t<commandSpeed>%lf</commandSpeed>\n", myBoat.commandArgs[1]);
		default:
			break;
	}
	switch(myBoat.commandStarted)
	{
		case true: fprintf(statusFile, "\t<started>1</started>\n"); break;
		case false: fprintf(statusFile, "\t<started>0</started>\n"); break;
	}
	fprintf(statusFile, "\t<lat>%lf</lat>\n", myBoat.current.fix.latitude);
	fprintf(statusFile, "\t<lon>%lf</lon>\n", myBoat.current.fix.longitude);
	fprintf(statusFile, "\t<compass>%lf</compass>\n", myBoat.retHeading);
	fprintf(statusFile, "\t<temp>%lf</temp>\n", myBoat.tempProbe);

	fprintf(statusFile, "</status>\n");
	fclose(statusFile);
	fprintf(stderr, "Finished writing status.\n");
}

int getCommand()
{
	fprintf(stderr, "Get Command.\n");
	FILE* fp = fopen("Commands/mission.txt", "r");
	if (fp == NULL)	{
		return 0;
	}	
	else {
		fclose(fp);
		myBoat.commandStarted = false;
		// Cleanup
		switch(myBoat.state){
			case WAYPOINT:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</waypoint>");
				break;
			case HEADING:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</heading>");
				break;
			case HALT:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</halt>");
				break;
			case ATSPEED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atspeed>");
				break;
			case ATHEADINGSPEED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atheadingspeed>");
				break;
			case ATHEADINGSPEEDMIXED:
				fprintf(logFile, "<comment>Interrupted.</comment>\n</atheadingspeedmixed>");
				break;
			default:
				break;
		}
		return 1;
	}
}	

int parseCommand()
{
	fprintf(stderr, "Parsing command.\n");
	rename( "Commands/mission.txt", "Commands/mission-parsing.txt");
	FILE* fp = fopen("Commands/mission-parsing.txt", "r");

	char str[MAX_LINE];
	while ((fgets(str, MAX_LINE, fp) != NULL))
	{
		//fprintf(stderr, "String: '%s' returned\n", str);
		char *pch;
		pch = strchr(str, '#');
		if (pch != NULL)
		{
			//fprintf(stderr, "Comment: %s", str);
		}
		else
		{
			int empty = strcmp(str, "\n");
			if (empty == 1)
			{
				char function[50];
				char args[50];

				sscanf(str, "%s \t %[^\n] \n", function, args);
				
				fprintf(stderr, "%s\n", args);
				if (!strcmp(function, "goToWaypoint"))
				{
					double lat = 1;
					double lon = 1;
					double precision = 1;// lon, precision;
					double speed = 1;
					sscanf(args, "[ %lf, %lf, %lf, %lf ]", &lat, &lon, &precision, &speed);
					//fprintf(stderr, "%f\n", lat);
					//fprintf(stderr, "Going to waypoint: %f,%f with %f precision, %f speed\n", lat, lon, precision, speed);
					
					myBoat.state = WAYPOINT;
					myBoat.commandArgs[0] = lat;
					myBoat.commandArgs[1] = lon;
					myBoat.commandArgs[2] = precision;
					myBoat.commandArgs[3] = speed;
					printLog();
					
				}
				else if (!strcmp(function, "goAtSpeed"))
				{
					double lat = 1;
					double lon = 1;
					double speed = 1;
					sscanf(args, "[ %lf, %lf, %lf ]", &lat, &lon, &speed);
					myBoat.state = ATSPEED;
					myBoat.commandArgs[0] = lat;
					myBoat.commandArgs[1] = lon;
					myBoat.commandArgs[2] = speed;
					printLog();
					
				}

				else if (!strcmp(function, "goHeadingSpeed"))
				{
					double heading = 0;
					double speed = 1;
					sscanf(args, "[ %lf, %lf]", &heading, &speed);
					myBoat.state = ATHEADINGSPEED;
					myBoat.commandArgs[0] = heading;
					myBoat.commandArgs[1] = speed;
					printLog();
					
				}

				else if (!strcmp(function, "goHeadingSpeedMixed"))
				{
					double heading = 0;
					double speed = 1;
					sscanf(args, "[ %lf, %lf]", &heading, &speed);
					myBoat.state = ATHEADINGSPEEDMIXED;
					myBoat.commandArgs[0] = heading;
					myBoat.commandArgs[1] = speed;
					printLog();
					
				}
				else if(!strcmp(function, "goToHeading"))
				{
					double desHeading;
					double speed = 1;
					sscanf(args, "[%lf, %lf]", &desHeading, &speed);
					//fprintf(stderr, "Going to desired heading of: %f\n", desHeading);

					myBoat.state = HEADING;
					myBoat.commandArgs[0] = desHeading;
					myBoat.commandArgs[1] = speed;
					printLog();
				}
				else if(!strcmp(function, "halt"))
				{
					myBoat.state = HALT;
					printLog();
				}
				else
				{
					fprintf(stderr, "cannot parse %s\n", function);
				}

			}
			else
			{
				//fprintf(stderr, "Empty line\n");
			}
		}
	}

	fclose(fp);


	rename( "Commands/mission-parsing.txt", "Commands/mission-running.txt");
	return 0;
}

double calcDistance(double lat1, double lon1, double lat2, double lon2)
{
	// Implemented using the haversine formula
	double radius = 637100;	

	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double dLat = (lat2-lat1);
	double dLon = (lon2-lon1);
		
	double a = sin(dLat/2) * sin(dLat/2) +
		 sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2);
	double c = 2 * atan2(sqrt(a), sqrt(1-a));

	fprintf(stderr, "Distance: %lf\n.", c*radius);

	return (c * radius);
}

double calcHeading(double lat1, double lon1, double lat2, double lon2)
{
	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double dLon = lon2-lon1;
	//double dLat = lat2-lat1;

	double y = sin(dLon) * cos(lat2);
	double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
	double desiredHeading = (atan2(y, x) * 180/M_PI);
	return fmod(desiredHeading + 360.0, 360);
}

void signal_handler(int signum)
{

	setTarget(maestro, getRudder(), 1500);
	setTarget(maestro, getThrottle(), 1500);
	switch(myBoat.state){
		case WAYPOINT:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</waypoint>");
			fclose(logFile);
			break;
		case HEADING:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</heading>");
			fclose(logFile);
			break;
		case HALT:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</halt>");
			fclose(logFile);			
			break;
		case ATSPEED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atspeed>");
			fclose(logFile);			
			break;
		case ATHEADINGSPEED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atheadingspeed>");
			fclose(logFile);			
			break;
		case ATHEADINGSPEEDMIXED:
			fprintf(logFile, "<comment>Signal Interrupted.</comment>\n</atheadingspeedmixed>");
			fclose(logFile);			
			break;
		default:
			break;
	}

	exit(signum);	
}

